/*

Routines to perform polygon scan conversion with z-buffer hidden surface
determination.  This version sorts the polygons in screen space and
processes the polygons in scanline order.

Greg Turk, October and December 1996, April 1997

*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "matlib.h"
#include <string.h>
#include "malloc.h"
#include "volscan.h"
#include "invert.h"
#include "polylib.h"
#include <assert.h>

int voxel_size_decision = XSIZE;

GregVoxelizer::GregVoxelizer(gtSimulator* gt) : global_num_verts(0), current_num_verts(0), global_max_verts(100), polys(NULL), num_polys(0), max_polys(100), zbuffer_enabled(0), gtSim(gt)
{
	filter_type = BOX_FILTER;
	filter_radius = 1;
	std_dev = 0.7;
	voxel_size = 50;
	supersample = 1;
	voxel_pad = 0;
	pgm_header = 1;     
	ray_stab = 0;
	double_surface_dist = 0.5;
}

GregVoxelizer::~GregVoxelizer()
{

}
/******************************************************************************
Read in polygons from a PLY file.

Entry:
  file - file to read polygons from
******************************************************************************/

//void read_polygons(FILE *file)
//{
//  int i,j;
//  int elem_count;
//  char *elem_name;
//  PlyFile *in_ply;
//  int old_num_verts,new_num_verts;
//  int old_num_polys,new_num_polys;
//
//  /*** Read in the original PLY object ***/
//
//  in_ply  = read_ply (file);
//
//  for (i = 0; i < in_ply->num_elem_types; i++) {
//
//    /* prepare to read the i'th list of elements */
//    elem_name = setup_element_read_ply (in_ply, i, &elem_count);
//
//    if (equal_strings ("vertex", elem_name)) {
//
//      old_num_verts = global_num_verts;
//      new_num_verts = global_num_verts + elem_count;
//
//      /* make sure we have room for the new vertices */
//
//      if (poly_verts == NULL) {
//        if (global_max_verts < new_num_verts)
//          global_max_verts = new_num_verts;
//        poly_verts = (gtVertex **) malloc(sizeof(gtVertex *)*global_max_verts);
//      }
//      else if (new_num_verts >= global_max_verts) {
//        global_max_verts += elem_count;
//        poly_verts = (gtVertex **)
//                 realloc (poly_verts, sizeof (gtVertex *) * global_max_verts);
//      }
//
//      /* set up for getting vertex elements */
//
//      setup_property_ply (in_ply, &vert_props[0]);
//      setup_property_ply (in_ply, &vert_props[1]);
//      setup_property_ply (in_ply, &vert_props[2]);
//
//      /* grab all the vertex elements */
//      for (j = old_num_verts; j < new_num_verts; j++) {
//
//        gtVertex *v = (gtVertex *) malloc (sizeof (gtVertex));
//        poly_verts[j] = v;
//
//        get_element_ply (in_ply, (void *) poly_verts[j]);
//      }
//
//      global_num_verts = new_num_verts;
//    }
//    else if (equal_strings ("face", elem_name)) {
//
//      old_num_polys = num_polys;
//      new_num_polys = num_polys + elem_count;
//
//      /* make sure we have enough room for the new polygons */
//
//      if (polys == NULL) {
//        if (max_polys < new_num_polys)
//          max_polys = new_num_polys;
//        polys = (gtPolygon **) malloc (sizeof(gtPolygon *) * max_polys);
//      }
//      else if (new_num_polys >= max_polys) {
//        max_polys += elem_count;
//        polys = (gtPolygon **) realloc (polys, sizeof(gtPolygon *) * max_polys);
//      }
//
//      /* set up for getting face elements */
//
//      setup_property_ply (in_ply, &face_props[0]);
//
//      /* grab all the face elements */
//      for (j = old_num_polys; j < new_num_polys; j++) {
//        polys[j] = (gtPolygon *) malloc (sizeof (gtPolygon));
//        get_element_ply (in_ply, (void *) polys[j]);
//        polys[j]->id = j;
//      }
//
//      num_polys = new_num_polys;
//    }
//    else
//      get_other_element_ply (in_ply);
//  }
//
//  close_ply (in_ply);
//  free_ply (in_ply);
//
//  /* translate vertex indices to pointers */
//
//  for (i = old_num_polys; i < new_num_polys; i++) {
//    gtPolygon *p = polys[i];
//    p->verts = (gtVertex **) malloc (sizeof (gtVertex *) * p->num_verts);
//    for (j = 0; j < p->num_verts; j++) {
//      p->verts[j] = poly_verts[old_num_verts + p->iverts[j]];
//    }
//    free (p->iverts);
//  }
//
//  /* map the vertices and their normals to eye space */
//
//  for (i = old_num_verts; i < new_num_verts; i++) {
//
//    gtVertex *v = poly_verts[i];
//    gtVector vec;
//
//    vec[0] = v->x;
//    vec[1] = v->y;
//    vec[2] = v->z;
//    apply_current_transformation (vec);
//    v->x = v->ex = vec[0];
//    v->y = v->ey = vec[1];
//    v->z = v->ez = vec[2];
//  }
//}





/******************************************************************************
Copy the contents of a source vertex to a destination vertex.
******************************************************************************/

void GregVoxelizer::vertex_copy (gtVertex *dest, gtVertex *source)
{
  //bcopy (source, dest, sizeof (gtVertex));
	memcpy(dest, source, sizeof (gtVertex));
}







/******************************************************************************
Check if polygon is not "horizontally convex" by counting the number of
times switch y directions as we travel around the polygon.

Entry:
  poly - polygon that has been transformed into screen space
  iv   - index of vertex with the minimum y value

Exit:
  returns number of times we switch y direction
******************************************************************************/

int GregVoxelizer::convexity_test (gtPolygon *poly, int iv)
{

#define UP    1
#define DOWN  2
#define HORIZ 3

  int i;
  int switches = 0;
  int dir = HORIZ;    /* acts as "don't care" */
  int new_dir;
  int num_verts = poly->num_clip_verts;

  for (i = 0; i < num_verts; i++) {

    float y0 = poly->clip_verts[(i + iv) % num_verts].y;
    float y1 = poly->clip_verts[(i + iv + 1) % num_verts].y;

    /* are we going up, down or horizontally ? */
    if (y0 < y1)
      new_dir = UP;
    else if (y0 > y1)
      new_dir = DOWN;
    else
      new_dir = HORIZ;

    /* see if we've switched direction */
    if (new_dir != HORIZ) {
      if (new_dir != dir && dir != HORIZ)
	switches++;
      dir = new_dir;
    }
  }

  return (switches);
}


/******************************************************************************
Determine if a point is inside a volume.

Entry:
  depth_buffer - data structure storing the volume
  x,y,z        - coordinates of point to test

Exit:
  returns 1 if inside, 0 if outside, BAD_SAMPLE if something is screwy
******************************************************************************/

double trunc(double value)
{
	if (value >= 0)
		return floor(value);
	else
		return ceil(value);
}

int GregVoxelizer::inside_volume(DepthBuffer *depth_buffer, float x, float y, float z)
{
  int i,j;
  int index;
  DepthSample *d;
  int in_flag = 0;

  i = (int) trunc(x+0.5);
  j = (int) trunc(y+0.5);

  if (i < 0 || i >= depth_buffer->xsize || j < 0 || j >= depth_buffer->ysize)
    return (0);

  index = j * depth_buffer->xsize + i;
  d = depth_buffer->depths[index];

  /* perform ray stab version? */

  if (ray_stab) {

    DepthSample *dnear = d;

    if (d == NULL)
      return (0);

    while (d->next != NULL)
      d = d->next;

    if (dnear == d)
      return (BAD_SAMPLE);
    else if (z > dnear->value && z < d->value)
      return (1);
    else
      return (0);
  }

  /* watch out for bad samples */
  if (d == (DepthSample *) BAD_SAMPLE)
    return (BAD_SAMPLE);

  /* alternating inside and outside */
  while (d != NULL && d->value < z) {
    in_flag = 1 - in_flag;
    d = d->next;
  }

  return (in_flag);
}


/******************************************************************************
Write one slice of a volume to a PGM file

Entry:
  xsize       - width of slice
  ysize       - height of slice
  slice_data  - the voxel values
  volume_name - prefix for PGM file names
  slice_num   - number of the current slice
******************************************************************************/

void GregVoxelizer::write_slice(
  int xsize,
  int ysize,
  unsigned char *slice_data,
  char *volume_name,
  int slice_num
)
{
  int i;
  FILE *fp;
  char filename[80];
  int result;

  /* open PGM file for writing */

  sprintf (filename, "%s%04d.pgm", volume_name, slice_num);
  fp = fopen (filename, "w" );

  if (fp == NULL) {
    fprintf (stderr, "Can't open PGM file '%s' for writing.\n", filename);
    exit (-1);
  }

  /* Maybe write header info.  (If not, the file is in "raw" format.) */

  if (pgm_header) {
    fprintf (fp, "P5\n");
    fprintf (fp, "%d %d\n", xsize, ysize);
    fprintf (fp, "%d\n", 255);
  }

  /* write the current slice to the PGM file */

  for (i = 0; i < ysize; i++) {

    int offset = i * xsize;

    /* write a row of voxels to the PGM file */
    result = fwrite (slice_data + offset, sizeof (unsigned char), xsize, fp);
    if (result != sizeof (unsigned char) * xsize) {
      fprintf (stderr, "Can't write to '%s'.\n", filename);
      exit (-1);
    }
  }

  fclose (fp);
}




/******************************************************************************
Filter a collection of voxel slices.

Caveat: For simplicity of coding, this routine does not perform any filtering
on the very edges of the slices.  Values here will be made zero, whether they
should be or not.

Entry:
  slices        - the voxel slices
  xs,zs         - width and height of the voxel slice to be created
  supersample   - amount of voxel supersampling
  filter_radius - radius of the interpolation (should be 1)

Exit:
  filtered - the filtered slice
******************************************************************************/

void GregVoxelizer::voxel_filter(
  unsigned char **slices,
  int xs,
  int zs,
  int supersample,
  int filter_radius,
  float *filtered
)
{
  int i;
  int x,y,z;
  int xsize = xs * supersample;
  int zsize = zs * supersample;
  int filter_width = (2 * filter_radius + 1) * supersample;
  float sum;
  float center;
  static float *filter_img1 = NULL;
  static float *filter_img2 = NULL;
  static float *filter_values;

  /* create the filter values (this is a separable filter, so 1D is enough) */

  filter_values = (float *) malloc (sizeof (float) * filter_width);

  sum = 0;
  center = 0.5 * filter_width;

  for (i = 0; i < filter_width; i++) {

    /* find distance to the center of the filter kernel */
    /* (distance equals one at the middle of the adjacent final voxel) */
    /* (furthest distance in adjacent voxel along an axis is 1.5) */

    float value;
    float pos = 0.5 + i;
    float dist = fabs (pos - center) / (float) supersample;

    /* select value based on filter type */

    if (filter_type == BOX_FILTER) {
      value = 1;
    }
    else if (filter_type == TENT_FILTER) {
      value = 1 - dist;
      if (value < 0)
        value = 0;
    }
    else if (filter_type == GAUSSIAN_FILTER) {
      value = exp (-0.5 * dist * dist / (std_dev * std_dev));
      if (value < 0)
        value = 0;
    }
    else {
      fprintf (stderr, "Don't know filter type %d\n", filter_type);
      exit (-1);
    }

    filter_values[i] = value;
    sum += filter_values[i];

#if 0
  if (filter_img2 == NULL)
    printf ("i = %d, dist = %f, filter_value = %f\n", i, dist, value);
#endif

  }

  /* normalize the filter values (integral of kernel is one) */

  for (i = 0; i < filter_width; i++)
    filter_values[i] /= sum;

  /* If necessary, create the temporary filter arrays */

  if (filter_img1 == NULL) {
    filter_img1 = (float *) malloc (sizeof(float) * xsize * zsize);
    filter_img2 = (float *) malloc (sizeof(float) * xs * zsize);
  }

  /* Zero out the temporary filtered values.  These are used */
  /* to contain values from the three separable filter passes. */

  for (i = 0; i < xsize * zsize; i++)
    filter_img1[i] = 0;

  for (i = 0; i < xs * zsize; i++)
    filter_img2[i] = 0;

  /* filter pass number one: weighted sum in "y" direction */
  /* (place result in "filter_img1") */

  for (y = 0; y < filter_width; y++) {

    float fvalue = filter_values[y];
    unsigned char *slice = slices[y];

    for (i = 0; i < xsize * zsize; i++)
      filter_img1[i] += slice[i] * fvalue;
  }

  /* filter pass number two: weighted sum in "x" direction */
  /* (place result in "filter_img2") */

  for (z = 0; z < zsize; z++) {

    for (x = filter_radius; x < xs - filter_radius; x++) {

      sum = 0;
      for (i = 0; i < filter_width; i++) {
        int xx = (x - filter_radius) * supersample + i;
        int index = xx * zsize + z;
        sum += filter_img1[index] * filter_values[i];
      }

      filter_img2[x * zsize + z] = sum;
    }
  }

  /* filter pass number three: weighted sum in "z" direction */
  /* (place result in "filtered") */

  for (x = 0; x < xs; x++) {

    for (z = filter_radius; z < zs - filter_radius; z++) {

      sum = 0;
      for (i = 0; i < filter_width; i++) {
        int zz = (z - filter_radius) * supersample + i;
        int index = x * zsize + zz;
        sum += filter_img2[index] * filter_values[i];
      }

      filtered[x * zs + z] = sum;
    }
  }

}


/******************************************************************************
Write volume data to a file.

Entry:
  buffers     - the depth buffers
  num_buffers - number of depth buffers (more than one to resolve ambiguities)
  volume_name - prefix for PGM file names
******************************************************************************/

void GregVoxelizer::write_volume(
  DepthBuffer **buffers,
  int num_buffers,
  char *volume_name
)
{
  int i,j,k,q;
  int index;
  DepthBuffer *depth_buffer = buffers[0]; /* first buffer drives I/O loops */
  int xsize = depth_buffer->xsize;
  int ysize = depth_buffer->ysize;
  int zsize = depth_buffer->zsize;
  int xs = xsize / supersample;
  int ys = ysize / supersample;
  int zs = zsize / supersample;
  DepthSample *d;
  unsigned char **slices;    /* current collection of slices (for filtering) */
  unsigned char *voxel_data; /* current slice we're making */
  unsigned char *slice_data; /* filtered slice */
  float *filtered;           /* floating point version of current slice */
  int x,y,z,xx,yy,zz;
  float recip;
  gtMatrix *mats;
  int nslices;
  int slice_num;

  /* create a bunch of matrices for transforming into the appropriate */
  /* buffers' coordinate systems */

  mats = (gtMatrix *) malloc (sizeof (gtMatrix) * num_buffers);

  for (i = 1; i < num_buffers; i++)
    mult_matrix (mats[i], buffers[i]->mat, buffers[0]->imat);

  /* write voxels to PGM files */

  /* allocate slices of the voxel data and zero it out */

  nslices = (2 * filter_radius + 1) * supersample;

  slices = (unsigned char **) malloc (sizeof (unsigned char *) * nslices);
  for (i = 0; i < nslices; i++) {
    slices[i] = (unsigned char *) malloc(sizeof (unsigned char) * xsize*zsize);
    for (j = 0; j < xsize*zsize; j++)
      slices[i][j] = 0;
  }

  /* make room for a filtered slice of voxels, both byte and float version */

  filtered = (float *) malloc (sizeof (float) * xs * zs);
  slice_data = (unsigned char *) malloc (sizeof (unsigned char) * xs * zs);

  /* create the slices through the volume one by one, */
  /* each scanline becomes a slice through the volume */

  for (j = 0; j < ysize + filter_radius * supersample; j++) {

    /* Rotate around the slices in the slice list.  The last */
    /* slice in the slice list (slices) will be the most recent. */

    voxel_data = slices[0];  /* shorthand for current slice */

    for (i = 0; i < nslices-1; i++)
      slices[i] = slices[i+1];
    slices[nslices-1] = voxel_data;

    /* make some empty slices if we're off the end of the volume, */
    /* otherwise get voxel data from scan-conversion */

    if (j >= ysize) {

      /* empty slice */
      for (i = 0; i < xsize * zsize; i++)
        voxel_data[i] = 0;

    }
    else {

      /* get voxel data from scan-conversion, */
      /* each pixel becomes a row of the current slice */

      for (i = 0; i < xsize; i++) {

        int offset = i * zsize;
        index = j * xsize + i;

        /* each depth value becomes a voxel in the current row */

        for (k = 0; k < zsize; k++) {

          int result;
          int in_sum = 0;
          int out_sum = 0;

          /* test in each buffer whether the position is inside or outside */

          for (q = 0; q < num_buffers; q++) {

            float tx,ty,tz;
            float x1,y1,z1;

            if (q == 0) {
              tx = i;
              ty = j;
              tz = k;
            }
            else {
              x1 = i;
              y1 = j;
              z1 = k;
              tx = x1 * mats[q][0][0] + y1 * mats[q][1][0] + z1 * mats[q][2][0]
                      + mats[q][3][0];
              ty = x1 * mats[q][0][1] + y1 * mats[q][1][1] + z1 * mats[q][2][1]
                      + mats[q][3][1];
              tz = x1 * mats[q][0][2] + y1 * mats[q][1][2] + z1 * mats[q][2][2]
                      + mats[q][3][2];
            }

            result = inside_volume (buffers[q], tx, ty, tz);

            if (result != BAD_SAMPLE) {
              if (result)
                in_sum++;
              else
                out_sum++;
            }
          }

#if 1
          if (ray_stab) {
            if (out_sum > 0)
              voxel_data[k+offset] = 0;
            else
              voxel_data[k+offset] = 255;
          }
          else
            if (in_sum > out_sum)
              voxel_data[k+offset] = 255;
            else
              voxel_data[k+offset] = 0;
#endif


#if 0
          if (num_buffers == 3)
            voxel_data[k+offset] = in_sum * 75;
          else
            voxel_data[k+offset] = in_sum * 18;
#endif

        }
      }
    }

    /* Are we at a multiple of "supersample"?  If so, we will need to */
    /* go on and filter several slices together and write out the */
    /* resulting slice. */

    slice_num = (j / supersample) - filter_radius;

    if ((j+1) % supersample != 0 || slice_num < 0)
      continue;

    /* filter the voxel slices */

    voxel_filter (slices, xs, zs, supersample, filter_radius, filtered);

    /* Pad the four edges of the filtered slice with zeros.  This could */
    /* be incorrect if we have non-zero voxels right up to the edges. */

    /* pad top and bottom edges */
    for (x = 0; x < xs; x++)
      for (z = 0; z < filter_radius; z++) {
        filtered[x * zs + z] = 0;
        filtered[x * zs + (zs - 1 - z)] = 0;
      }

    /* pad left and right edges */
    for (z = 0; z < zs; z++)
      for (x = 0; x < filter_radius; x++) {
        filtered[x * zs + z] = 0;
        filtered[(xs - 1 - x) * zs + z] = 0;
      }

    /* convert floating point density to bytes */

    for (i = 0; i < xs * zs; i++)
      slice_data[i] = (int) filtered[i];

	if (j >= ys)
		continue;
	for (x = 0; x < xs; x++)
	{
		for (z = 0; z < zs; z++)
		{
			if (slice_data[x * zs + z] >= 50)
				mOutput[x][j][z] = 1;
			else
				mOutput[x][j][z] = 0;
		}
	}

    /* write the slice to a PGM file */

    //write_slice (zs, xs, slice_data, volume_name, slice_num);
  }

  /* free up memory */

  for (i = 0; i < nslices; i++)
    free (slices[i]);

  free (slices);
  free (filtered);
}


/******************************************************************************
Return transformation information after determining the size of an object
in each dimension.

Exit:
  d        - depth buffer information, including transformations
  decision - how to decide voxel size
  delta    - size of voxel (only meaningful for particular decisions)
******************************************************************************/

void GregVoxelizer::trans_from_size(DepthBuffer *d, int decision, float delta)
{
  int i;
  //float xmin = 1e20;
  //float xmax = -1e20;
  //float ymin = 1e20;
  //float ymax = -1e20;
  //float zmin = 1e20;
  //float zmax = -1e20;
  //float x,y,z;
  //float xx,yy,zz;
  float dx,dy,dz;
  float xscale,xtrans;
  float yscale,ytrans;
  float zscale,ztrans;
  int xpadsize,ypadsize,zpadsize;
  gtMatrix mat;

  /* find min and max values in x, y and z directions */

  //for (i = 0; i < global_num_verts; i++) {
  //  xx = poly_verts[i]->x;
  //  yy = poly_verts[i]->y;
  //  zz = poly_verts[i]->z;
  //  x = xx * d->mat[0][0] + yy * d->mat[1][0] + zz * d->mat[2][0] +d->mat[3][0];
  //  y = xx * d->mat[0][1] + yy * d->mat[1][1] + zz * d->mat[2][1] +d->mat[3][1];
  //  z = xx * d->mat[0][2] + yy * d->mat[1][2] + zz * d->mat[2][2] +d->mat[3][2];
  //  if (xmin > x) xmin = x;
  //  if (xmax < x) xmax = x;
  //  if (ymin > y) ymin = y;
  //  if (ymax < y) ymax = y;
  //  if (zmin > z) zmin = z;
  //  if (zmax < z) zmax = z;
  //}

  dx = vol_x_max - vol_x_min;
  dy = vol_y_max - vol_y_min;
  dz = vol_z_max - vol_z_min;

  /* select which coordinate will dictate voxel size */

  switch (decision) {
    case XSIZE:

      d->xsize = voxel_size;
      xpadsize = d->xsize - 2 * voxel_pad;
      d->xdelta = dx / (xpadsize);

      d->ydelta = d->xdelta;
      d->zdelta = d->xdelta;

      ypadsize = dy / d->ydelta;
      zpadsize = dz / d->zdelta;

      d->ysize = ypadsize + 2 * voxel_pad;
      d->zsize = zpadsize + 2 * voxel_pad;

	  assert(d->ysize == voxel_y_size);
	  assert(d->zsize == voxel_z_size);

      break;
    case YSIZE:
      break;
    case ZSIZE:
      break;
    case MAX_SIZE:
      break;
    case DELTA_SIZE:
      d->xdelta = delta;
      d->ydelta = delta;
      d->zdelta = delta;

      xpadsize = dx / d->xdelta;
      ypadsize = dy / d->ydelta;
      zpadsize = dz / d->zdelta;

      d->xsize = xpadsize + 2 * voxel_pad;
      d->ysize = ypadsize + 2 * voxel_pad;
      d->zsize = zpadsize + 2 * voxel_pad;

      break;
    default:
      printf ("not valid value, voxel_size_decision = %d\n",
              voxel_size_decision);
      break;
  }

  xscale = xpadsize / dx;
  xtrans = voxel_pad - 0.5 - vol_x_min * xpadsize / dx;
  yscale = ypadsize / dy;
  ytrans = voxel_pad - 1.5 - vol_y_min * ypadsize / dy;
  zscale = zpadsize / dz;
  ztrans = voxel_pad - 0.5 - vol_z_min * zpadsize / dz; 

  d->xsize *= supersample;
  d->ysize *= supersample;
  d->zsize *= supersample;

  xscale *= supersample;
  yscale *= supersample;
  zscale *= supersample;

  xtrans *= supersample;
  ytrans *= supersample;
  ztrans *= supersample;

  identity_matrix (mat);

  mat[0][0] = xscale;
  mat[1][1] = yscale;
  mat[2][2] = zscale;

  mat[3][0] = xtrans;
  mat[3][1] = ytrans;
  mat[3][2] = ztrans;

  mult_matrix (d->mat, mat, d->mat);

  /* compute the inverse matrix */
  copy_matrix (d->imat, d->mat);
  mat_invert (d->imat);

#if 1
  fprintf (stderr, "xsize ysize zsize: %d %d %d\n",
           d->xsize, d->ysize, d->zsize);
#endif

#if 0
  printf ("xmin xmax: %f %f\n", xmin, xmax);
  printf ("ymin ymax: %f %f\n", ymin, ymax);
  printf ("zmin zmax: %f %f\n", zmin, zmax);
  printf ("dx dy dz: %f %f %f\n", dx, dy, dz);
  printf ("xsize ysize zsize: %d %d %d\n", d->xsize, d->ysize, d->zsize);
  printf ("xdelta ydelta zdelta: %f %f %f\n", d->xdelta, d->ydelta, d->zdelta);
  printf ("xscale xtrans: %f %f\n", xscale, xtrans);
  printf ("yscale ytrans: %f %f\n", yscale, ytrans);
  printf ("zscale ztrans: %f %f\n", zscale, ztrans);
  printf ("xyz padding: %f %f %f\n", xpadsize, ypadsize, zpadsize);
  printf ("full transformation matrix:\n");
  print_matrix (d->mat);
  printf ("\n");
#endif

}


/******************************************************************************
Scan convert a polygonal model into a volume buffer.
******************************************************************************/

void GregVoxelizer::scan_volume(DepthBuffer *dbuffer)
{
  int i;
  int y;
  gtPolygon **scanlines;        /* scanlines we'll bucket sort into */
  gtPolygon *active_polys;      /* the active polygon list */
  gtPolygon *active_copy;
  gtPolygon *poly,*next_poly;
  float xs,xt,ys,yt,zs,zt;
  int xsize = dbuffer->xsize;
  int ysize = dbuffer->ysize;
  int dsize = xsize * ysize;
  DepthSample *ptr;
  int count;

  DepthSample *old_ptr;
  float depth,old_depth;

  /* set the global pointer to this depth buffer */
  
  current_depth_buffer = dbuffer;

  /* Set up all the initial edges and such of each polygon. */
  /* This also performs transformation into dbuffer's coordinate space. */

  for (i = 0; i < num_polys; i++)
    polygon_vol_setup (polys[i], dbuffer);

  /* allocate depth buffer for volume */

  dbuffer->depths = (DepthSample **) malloc (sizeof (DepthSample *) * dsize);
  for (i = 0; i < dsize; i++)
    dbuffer->depths[i] = NULL;

  /* bucket sort the polygons based on the 1st scanline on which they appear */

  /* allocate scanline lists */
  scanlines = (gtPolygon **) malloc (sizeof(gtPolygon*) * ysize);
  for (i = 0; i < ysize; i++)
    scanlines[i] = NULL;

  /* place all polygons on their appropriate scanline list */
  for (i = 0; i < num_polys; i++) {

    poly = polys[i];
    if (!poly->draw_flag) /* don't add to any list if it should not be drawn */
      continue;

    y = poly->ymin;
	poly->next = scanlines[y];
	scanlines[y] = poly;
  }


  /* scan-convert the polygons */
  /* (we keep an active polygon list that is updated at each scanline) */

  active_polys = NULL;  /* no active polygons to begin with */
  for (y = 0; y < ysize; y++) {

    /* add all polygons on this scanline to the active polygon list */

    for (poly = scanlines[y]; poly != NULL; poly = next_poly) {
      next_poly = poly->next;
      poly->next = active_polys;
      active_polys = poly;
    }

    /* draw the spans of all the active polygons */

    for (poly = active_polys; poly != NULL; poly = poly->next)
      render_span (poly, (float) y);

    /* update the active polygon list by selectively copying the list */

    active_copy = active_polys;
    active_polys = NULL;

    for (poly = active_copy; poly != NULL; poly = next_poly) {

      next_poly = poly->next;

      /* only copy to new list those polygons we're still drawing */
      if (poly->draw_flag) {
        poly->next = active_polys;
	active_polys = poly;
      }
    }

  }  /* for y */

  /* free up memory */
  free (scanlines);

  /* maybe discard distance samples that are too close to one another */

  if (double_surface_dist != 0)
    for (i = 0; i < dsize; i++) {

      old_ptr = NULL;

      for (ptr = dbuffer->depths[i]; ptr != NULL; ptr = ptr->next) {

        depth = ptr->value;

        if (old_ptr && fabs (depth - old_depth) < double_surface_dist) {
          old_ptr->next = ptr->next;
        }

        old_depth = depth;
        old_ptr = ptr;
      }

    }

  /* mark all the pixel locations with an odd number of samples as "bad" */

  if (!ray_stab)
    for (i = 0; i < dsize; i++) {

      /* count the depth samples at this pixel */
      count = 0;
      for (ptr = dbuffer->depths[i]; ptr != NULL; ptr = ptr->next)
        count++;

      if (count % 2 == 1)
        dbuffer->depths[i] = (DepthSample *) BAD_SAMPLE;
    }
}


/******************************************************************************
Create three depth buffers, each oriented along a different axis.
******************************************************************************/

DepthBuffer ** GregVoxelizer::three_buffers()
{
  int i;
  DepthBuffer **buffers;

  /* allocate space for the depth buffers */

  buffers = (DepthBuffer **) malloc (sizeof (DepthBuffer *) * 3);
  for (i = 0; i < 3; i++)
    buffers[i] = (DepthBuffer *) malloc (sizeof (DepthBuffer));

  /* create the transformation matrices for the depth buffers */

  identity_matrix (buffers[0]->mat);

  gtSim->gtPushMatrix();
  gtSim->gtRotate (90.0, 1.0, 0.0, 0.0);
  gtSim->get_top_matrix (buffers[1]->mat);
  gtSim->gtPopMatrix();

  gtSim->gtPushMatrix();
  gtSim->gtRotate (90.0, 0.0, 1.0, 0.0);
  gtSim->get_top_matrix (buffers[2]->mat);
  gtSim->gtPopMatrix();

  return (buffers);
}


/******************************************************************************
Create a number of depth buffers for scan-conversion along different
directions.
******************************************************************************/

DepthBuffer ** GregVoxelizer::many_buffers()
{
  int i,j;
  DepthBuffer **buffers;
  float dirs[] = {
    0, 0.934172, 0.356822,
    0, -0.934172, 0.356822,
    0.934172, 0.356822, 0,
    0.934172, -0.356822, 0,
    0.356822, 0, -0.934172,
    0.356822, 0, 0.934172,
    0.57735, 0.57735, -0.57735,
    0.57735, 0.57735, 0.57735,
    0.57735, -0.57735, -0.57735,
    0.57735, -0.57735, 0.57735,
  };
  int num_dirs = 13;

  /* allocate space for the depth buffers */

  buffers = (DepthBuffer **) malloc (sizeof (DepthBuffer *) * num_dirs);
  for (i = 0; i < num_dirs; i++)
    buffers[i] = (DepthBuffer *) malloc (sizeof (DepthBuffer));

  /* create the transformation matrices for the depth buffers */

  /* first create three along the major axes */

  identity_matrix (buffers[0]->mat);

  gtSim->gtPushMatrix();
  gtSim->gtRotate (90.0, 1.0, 0.0, 0.0);
  gtSim->get_top_matrix (buffers[1]->mat);
  gtSim->gtPopMatrix();

  gtSim->gtPushMatrix();
  gtSim->gtRotate (90.0, 0.0, 1.0, 0.0);
  gtSim->get_top_matrix (buffers[2]->mat);
  gtSim->gtPopMatrix();

  /* now create ten parallel to faces of icosahedron */

  for (i = 0; i < 10; i++) {
    j = (i+1) % 10;
    gtSim->gtPushMatrix();

    gtSim->gtLookAt (dirs[i*3], dirs[i*3+1], dirs[i*3+2], 0.0, 0.0, 0.0,
              dirs[j*3], dirs[j*3+1], dirs[j*3+2]);

    gtSim->get_top_matrix (buffers[i+3]->mat);
    gtSim->gtPopMatrix();
  }

  return (buffers);
}


/******************************************************************************
Render the scene.

Entry:
  volume_name - prefix of PGM files to write the volume to
  nbuffs      - number of depth buffers to create
******************************************************************************/

void GregVoxelizer::gtRenderScene(char *volume_name, int nbuffs)
{
  int i,k;
  int num_buffers;
  DepthBuffer **buffers;

  /* allocate depth buffers */

  if (nbuffs == 3) {
    buffers = three_buffers();
    num_buffers = 3;
  }
  else if (nbuffs == 13) {
    buffers = many_buffers();
    num_buffers = 13;
  }
  else {
    fprintf (stderr, "%d not a valid number of buffers\n", nbuffs);
    exit (-1);
  }

  /* create all the depth buffers */

  for (i = 0; i < num_buffers; i++) {

    /* determine depth buffer size */

    if (i == 0) {
      trans_from_size (buffers[i], voxel_size_decision, 0.0);
      draw_surfaces = 1;
    }
    else {
      trans_from_size (buffers[i], DELTA_SIZE, buffers[0]->xdelta);
      draw_surfaces = 0;
    }

    /* scan convert the polygons into the depth buffer */
    scan_volume (buffers[i]);
  }

  /* write voxel data to file */

  write_volume (buffers, num_buffers, volume_name);

  /* free up voxel data */

  for (k = 0; k < num_buffers; k++) {

    DepthSample *ptr,*next_ptr;

    for (i = 0; i < buffers[k]->xsize * buffers[k]->ysize; i++)
      if (buffers[k]->depths[i] != (DepthSample *) BAD_SAMPLE)
        for (ptr = buffers[k]->depths[i]; ptr != NULL; ptr = next_ptr) {
          next_ptr = ptr->next;
          free (ptr);
        }

    free (buffers[k]->depths);
    free (buffers[k]);
  }

  free (buffers);
}


/******************************************************************************
Prepare a polygon for scan conversion into a volume.

Entry:
  poly         - polygon to make ready
  depth_buffer - info about transforming into depth buffer's space
******************************************************************************/


void GregVoxelizer::polygon_vol_setup(
  gtPolygon *poly,
  DepthBuffer *depth_buffer
)
{
  int i,j;
  float y;
  int iv;
  gtVector v;
  int num_verts = poly->num_verts;
  gtVertex **verts,*cverts;
  gtVector vec;

  /* transform vertices and put into polygon structure */

  poly->clip_verts = (gtVertex *) malloc (sizeof(gtVertex) * num_verts);
  poly->num_clip_verts = num_verts;

  verts = poly->verts;
  cverts = poly->clip_verts;

  for (i = 0; i < num_verts; i++) {

    vertex_copy (&cverts[i], verts[i]);

    vec[0] = verts[i]->x;
    vec[1] = verts[i]->y;
    vec[2] = verts[i]->z;
    apply_matrix (vec, depth_buffer->mat);
    cverts[i].x = vec[0];
    cverts[i].y = vec[1];
    cverts[i].z = vec[2];
  }

  /* find minimum vertex in y */

  iv = 0;
  y = poly->clip_verts[0].y;

  for (i = 1; i < num_verts; i++)
    if (y > poly->clip_verts[i].y) {
      iv = i;
      y = poly->clip_verts[i].y;
    }

  y = ceil (y);


  poly->ymin = y;

  if (convexity_test (poly, iv) > 1) {
    fprintf (stderr, "Polygon is not horizontally convex (not drawn).\n");
    poly->draw_flag = 0;
    return;
  }
  if (poly->ymin < 0 || poly->ymin >= voxel_y_size)
  {
	  poly->draw_flag = 0;
	  return;
  }
  /* set up initial left and right edges */
  poly->ileft = iv;
  poly->iright = iv;
  setup_edge (&poly->left, y, poly->ileft, poly->ileft+1, poly);
  setup_edge (&poly->right, y, poly->iright, poly->iright-1, poly);

  poly->edges_remaining = num_verts - 2;

  poly->draw_flag = 1;
}


/******************************************************************************
Set up the information for an edge.

Entry:
  edge  - edge to set up
  y     - y coordinate of scanline to start at
  i,j   - indices into poly->verts for the two vertices of the edge
  poly  - the current polygon
******************************************************************************/

void GregVoxelizer::setup_edge (Edge *edge, float y, int i, int j, gtPolygon *poly)
{
  float y0,y1;
  float dy_recip,fract;
  float *p0,*p1,*p,*dp;
  gtVertex *v0,*v1;
  int num_verts = poly->num_clip_verts;

  /* make sure i and j are in the range [0,num_verts-1] */

  if (i >= num_verts)
    i -= num_verts;
  if (i < 0)
    i += num_verts;

  if (j >= num_verts)
    j -= num_verts;
  if (j < 0)
    j += num_verts;

  /* make sure that v0 has the lower of the two y coordinates */

  if (poly->clip_verts[i].y < poly->clip_verts[j].y) {
    v0 = &poly->clip_verts[i];
    v1 = &poly->clip_verts[j];
  }
  else {
    v0 = &poly->clip_verts[j];
    v1 = &poly->clip_verts[i];
  }

  y0 = v0->y;
  y1 = v1->y;
  edge->ymax = y1;

  /* we should ignore horizontal edges */

  if (y0 == y1)
    return;

  dy_recip = 1.0 / (y1 - y0);
  fract = (y - y0) * dy_recip;

  /* compute initial and delta values for all interpolated parameters */

  p0 = FIRST (v0);
  p1 = FIRST (v1);
  p  = FIRST (&edge->point);
  dp = FIRST (&edge->delta);

  for (i = 0; i < NUM_ATTRIBS; i++, p++, p0++, p1++, dp++) {
    *p = (*p0) + fract * ((*p1) - (*p0));
    *dp = ((*p1) - (*p0)) * dy_recip;
  }
}


/******************************************************************************
Render one span of a polygon on a given scanline.

Entry:
  poly - polygon whose span is to be rendered
  y    - position of scanline that we're rendering on
******************************************************************************/

void GregVoxelizer::render_span(gtPolygon *poly, float y)
{
  int i;
  float *rt,*lt,*dr,*dl;

  /* update the left and right edges of the polygon */

  if (poly->edges_remaining > 0) {

    while (poly->left.ymax <= y && poly->edges_remaining > 0) {
      poly->ileft++;
      setup_edge (&poly->left, y, poly->ileft, poly->ileft+1, poly);
      poly->edges_remaining--;
    }

    while (poly->right.ymax <= y && poly->edges_remaining > 0) {
      poly->iright--;
      setup_edge (&poly->right, y, poly->iright, poly->iright-1, poly);
      poly->edges_remaining--;
    }

  }

  /* render the span of the polygon, or mark it as finished */

  if (y < poly->left.ymax && y < poly->right.ymax) {

    global_poly = poly;
    fill_scanline (y, &poly->left, &poly->right);

    /* add delta values to all interpolated parameters along the left */
    /* and right edges */

    lt = FIRST (&poly->left.point);
    dl = FIRST (&poly->left.delta);
    rt = FIRST (&poly->right.point);
    dr = FIRST (&poly->right.delta);

    for (i = 0; i < NUM_ATTRIBS; i++, lt++, rt++, dl++, dr++) {
      *lt += *dl;
      *rt += *dr;
    }
  }
  else
    poly->draw_flag = 0;  /* signal that we're done with this polygon */
}


/******************************************************************************
Fill a scanline with pixels between two edges.

Entry:
  y     - vertical position of scanline to write to
  e0,e1 - the two edges to fill between
******************************************************************************/

void GregVoxelizer::fill_scanline (float y, Edge *e0, Edge *e1)
{
  int i;
  int x;
  int xinit;
  float x0,x1;
  float dx_recip;
  float fract;
  gtVertex point,delta;
  float *p0,*p1,*p,*dp;
  float r,g,b;

  /* make sure edges are ordered left to right */

  if (e0->point.x > e1->point.x) {
    Edge *etemp;
    etemp = e0;
    e0 = e1;
    e1 = etemp;
  }

  x0 = e0->point.x;
  x1 = e1->point.x;

  if (x0 == x1)
    return;

  /* set up things for interpolation across scanline */

  xinit = (int) ceil(x0);
  dx_recip = 1.0 / (x1 - x0);
  fract = (xinit - x0) * dx_recip;

  /* compute initial values (p) and delta values (dp) for interpolating */
  /* the values across the scanline */

  p0 = FIRST (&e0->point);
  p1 = FIRST (&e1->point);
  p  = FIRST (&point);
  dp = FIRST (&delta);

  for (i = 0; i < NUM_ATTRIBS; i++, p++, p0++, p1++, dp++) {
    *p = (*p0) + fract * ((*p1) - (*p0));
    *dp = ((*p1) - (*p0)) * dx_recip;
  }

  /* loop over the pixel in the span */

  for (x = xinit; x < x1; x++) {

    /* write the depth value into voxel buffer */
	if (x >= voxel_size || x < 0)
		continue;
    int placed = 0;
    int index = y * current_depth_buffer->xsize + x;
    DepthSample *ptr,*last_ptr;

    /* create new depth sample */

    DepthSample *d = (DepthSample *) malloc (sizeof(DepthSample));

    d->value = point.z;
    d->id = global_poly->id;
    d->next = NULL;

    /* insert new sample into the linked list of samples at this location */

    last_ptr = NULL;
    ptr = current_depth_buffer->depths[index];

    while (!placed) {
      if (ptr == NULL || ptr->value > d->value) {
        d->next = ptr;
        if (last_ptr)
          last_ptr->next = d;
        else
          current_depth_buffer->depths[index] = d;
        placed = 1;
      }
      else {
        last_ptr = ptr;
        ptr = ptr->next;
      }
    }

    /* update all interpolated values (z, color, etc.) */

    p  = FIRST (&point);
    dp = FIRST (&delta);

    for (i = 0; i < NUM_ATTRIBS; i++, p++, dp++)
      *p += *dp;
  }
}


void GregVoxelizer::SetGeomtryInfo(gtVertex** polyVerts, int numVerts, gtPolygon** polygons, int numPolys)
{
	poly_verts = polyVerts;       /* vertex list */
	global_num_verts = numVerts;
	
	polys = polygons;    /* list of polygons */
	num_polys = numPolys;
	
}

void GregVoxelizer::SetParamsAndOutput(int xSize, int ySize, int zSize, double volMinX, double volMinY, double volMinZ, double volMaxX, double volMaxY, double volMaxZ, BYTE*** solidElements)
{
	mOutput = solidElements;
	voxel_size = xSize;
	voxel_y_size = ySize;
	voxel_z_size = zSize;

	vol_x_min = volMinX;
	vol_x_max = volMaxX;
	vol_y_min = volMinY;
	vol_y_max = volMaxY;
	vol_z_min = volMinZ;
	vol_z_max = volMaxZ;
}